// #include <std_msgs/String.h>
#include <iostream>
#include<fstream>
#include<vector>
#include <unistd.h>
struct way_point
{
    double x;
    double y;
    double z;
    double time;
}wp_;

int main()
{
    std::vector<way_point> points;
    std::ifstream infile("./way_points.txt");
    while(!infile.eof() ){
        double x ,y, z, t;
        infile >> wp_.x;
        infile >> wp_.y;
        infile >> wp_.z;
        infile >> wp_.time;
        points.push_back(wp_);
        std::cout << "read back from file: " << wp_.x << ' ' << wp_.y << ' ' 
            << wp_.z  << ' ' << wp_.time << '\n';
    }


    
    std::cout<<"arm" <<std::endl;
    sleep(2);
    std::cout<<"take off" <<std::endl;
    sleep(2);
     for (size_t i = 0; i < points.size(); ++i)
    {
               std::cout << "current pose: " << points[i].x << ' ' << points[i].y << ' ' 
            <<  points[i].z  << ' ' <<  points[i].time << '\n'; 
            std::string test_s;
            
            sleep(points[i].time);  
            if(i == points.size()-1){
                std::cout<<"land" <<std::endl;
            }          
    }
   











    // infile >> i_params.lidar_topic;
    // infile >> i_params.fisheye_model;
    // infile >> i_params.lidar_ring_count;
}